#include "baseband.h"
#include "ts_peripheral.h"
#include "advertise.h"
#include "slave_dispatch.h"

static uint8_t crcInit[3];
static uint8_t AA[4];
static uint8_t winSize;
static uint16_t winOffset;
static uint16_t interval, latency, timeout;
static uint8_t chM[5];

static uint8_t hop, sca;
static uint8_t supervision_timeout;
static uint8_t ch, unmapped_ch;
static uint8_t remapping_table[37];
static uint8_t num_used_channels;
static uint8_t numUnUseCh;
static uint8_t pdu_type;

static uint8_t ch_sign,timer3_sign;
//static uint8_t m_tx_data[39] = {0x0B,0x01,0x01,0x01};
static uint8_t m_tx_data[39] = {0x01,0x00};
static uint8_t _m_tx_first[2] = {0x0D,0x00};
static uint8_t tx_data_sign;
//static uint8_t m_tx_data[39] = {0x03,0x06,0x0C,0x07,0x0F,0x00,0x0D,0x41};
static uint8_t m_rx_data[39] = {0};

static void mconn_onNumUsedCh(void);
static void mconn_onWakeup(void);
static void mconn_onRxAA(void);
static void mconn_onRxPDU( void);
static void mconn_onTxStarted( void );
static void mconn_onSleep( void );
static void set_pdu(uint8_t * pdu, uint8_t len);
static void apply_connPara(uint8_t * pdu, uint8_t len);

static const bb_fsm_t mconnFsm = {
    1,
    NULL,
    mconn_onWakeup,
    mconn_onRxAA,
    mconn_onRxPDU,
    mconn_onTxStarted,
    mconn_onSleep,
		set_pdu
};
void bb_mconnStart(uint8_t* pdu, uint8_t len, uint32_t hfclk)
{		
		pdu_type = pdu[0] &0x0F;
		bb_fsm = &mconnFsm;
		apply_connPara( pdu, len );
		//periph_radio_packet_ptr_set((uint8_t*)&m_tx_data[0]);
    /*the number of used channel*/
    mconn_onNumUsedCh();
	
    radio_setCrcInit(crcInit);
    radio_setAccessCode(AA);
		
		 // setup next channel
    unmapped_ch += hop;
    unmapped_ch %= 37;
    if (chM[unmapped_ch/8] &( 1 << (unmapped_ch%8))) {
        ch = unmapped_ch;
    } else {
        ch = remapping_table[unmapped_ch % num_used_channels];
    }
		if(ch <= 10){
    	NRF_RADIO->FREQUENCY = 4 + (ch*2);
			NRF_RADIO->DATAWHITEIV = ch;
		}else{
			NRF_RADIO->FREQUENCY = 6 + (ch*2);
			NRF_RADIO->DATAWHITEIV = ch;
		}
		ch_sign = 1;
		//periph_radio_packet_ptr_set((uint8_t*)&m_tx_data[0]);
	
}
static void mconn_onWakeup(void)
{
	
	//if(bb_getConnRole() == MASTER_STATE){
	if(timer3_sign < 6){
		NRF_TIMER0->CC[0]=(NRF_TIMER0->CC[1] +(interval*1250))% 65536;
		NRF_TIMER0->CC[2]=(NRF_TIMER0->CC[1] +(interval*1250) - 500)% 65536;
		//NRF_TIMER0->CC[3]=(NRF_TIMER0->CC[1] +(interval*1250) +600)% 65536;
		NRF_TIMER0->CC[3]=(NRF_TIMER0->CC[1] +(interval*1250) +100)% 65536;
		timer3_sign++;
	}else{
		NRF_TIMER0->CC[0]=(NRF_TIMER0->CC[0] +(interval*1250))% 65536;
		NRF_TIMER0->CC[2]=(NRF_TIMER0->CC[2] +(interval*1250))% 65536;
		//NRF_TIMER0->CC[3]=(NRF_TIMER0->CC[1] +(interval*1250) +600)% 65536;
		NRF_TIMER0->CC[3]=(NRF_TIMER0->CC[3] +(interval*1250))% 65536;
	}
		
	//}
	
}

static void mconn_onTxStarted( void )
{
	periph_radio_packet_ptr_set((uint8_t*)&m_tx_data[0]);

}

static void mconn_onRxAA(void)
{
	
	supervision_timeout = 0;
	
}

static void mconn_onRxPDU(void)
{
	 m_opcodeCheck((uint8_t*)&m_rx_data[0], m_rx_data[1] + 3);
}

static void mconn_onSleep( void )
{
	if(bb_getConnRole() != M_CONN_REQ){
		periph_radio_packet_ptr_set((uint8_t*)&m_rx_data[0]);
		
	}else{
		NRF_TIMER0->CC[0]=(NRF_TIMER0->CC[1] + (1250 + (winOffset*1250))+1250*(winSize/2) + 161) % 65536;
		NRF_TIMER0->CC[2]=(NRF_TIMER0->CC[1] + 410) % 65536;
		NRF_TIMER0->CC[3]= (NRF_TIMER0->CC[1]+(1250 + (winOffset*1250)+1250*(winSize/2) +(interval*1250)+800 + 161)) % 65536;
		periph_radio_packet_ptr_set((uint8_t*)&m_tx_data[0]);
		PERIPHERAL_TASK_TRIGGER(NRF_RADIO->TASKS_TXEN);
		
	}
	//set_ch();
}

/*apply connection request pdu*/
static void apply_connPara(uint8_t * pdu, uint8_t len)
{
		 uint8_t* lldata;
    /*
      CONNECT_REQ PDU Format
      InitA (6 octets)
      AdvA (6 octets)
      LLData (22 octets)

      LLData

      AA (4 octets)
      CRCInit (3 octets)
      WinSize (1 octet)
      WinOffset (2 octets)
      Interval (2 octets)
      Latency (2 octets)
      Timeout (2 octets)
      ChM (5 octets)
      Hop (5 bits)
      SCA (3 bits)

    */
		//uart_put_rx_data((uint8_t*)&pdu[0],len);
		 lldata = pdu + 15;
		
    // byte 0
    memcpy(AA, lldata, 4);
		//uart_put_rx_data(AA,4);
    // byte 4
    memcpy(crcInit, lldata + 4, 3);
		//uart_put_rx_data(crcInit,3);
    // byte 7
    winSize = lldata[7];
    // byte 8
    winOffset = lldata[8] + (lldata[9] << 8);
    // byte 10
    interval = lldata[10] + (lldata[11] << 8);
    // byte 12
    latency = lldata[12] + (lldata[13] << 8);
    // byte 14
    timeout = lldata[14] + (lldata[15] << 8);

    memcpy(chM, lldata + 16, 5);
		
		 hop = lldata[21] & 0x1F;
		 sca = lldata[21] >> 5;
		
}

/*the number of used channel*/
static void mconn_onNumUsedCh(void)
{
		uint8_t i;
		unmapped_ch = 0;
    num_used_channels = 0;    
    for (i=0; i<37; i++) {
        if (chM[i/8] & (1 << (i%8))) {
            remapping_table[num_used_channels] = i;
            num_used_channels++;
        } else {
            numUnUseCh = i;
        }	
    }
}

void m_set_ch(void)
{
	if(ch_sign != 1){
		unmapped_ch += hop;
    unmapped_ch %= 37;
    if (chM[unmapped_ch/8] &( 1 << (unmapped_ch%8))) {
        ch = unmapped_ch;
    } else {
        ch = remapping_table[unmapped_ch % num_used_channels];
    }
    	if(ch <= 10){
    	NRF_RADIO->FREQUENCY = 4 + (ch*2);
			NRF_RADIO->DATAWHITEIV = ch;
		}else{
			NRF_RADIO->FREQUENCY = 6 + (ch*2);
			NRF_RADIO->DATAWHITEIV = ch;
		}
	}
			PERIPHERAL_TASK_TRIGGER(NRF_RADIO->TASKS_TXEN);

		ch_sign = 2;
		//NRF_TIMER0->CC[2]+= 100;
	
}

void m_set_interval(void)
{
		supervision_timeout++;
		if(supervision_timeout == 8){
			//adv_start();
			//supervision_timeout = 0;
			//NVIC_SystemReset();
		}
		NRF_TIMER0->CC[1] = ((NRF_TIMER0->CC[1]+(interval*1250)+37) % 65536);
		/*if((NRF_TIMER0->CC[1]+(interval*1250)) < 65536){
			NRF_TIMER0->CC[1]+=((interval*1250));
		}else{
			NRF_TIMER0->CC[1] = (NRF_TIMER0->CC[1]+(interval*1250)) - 65535;
		}	*/	
		//NRF_TIMER0->CC[0]=NRF_TIMER0->CC[0]+(interval*1250);
		
}

static void set_pdu(uint8_t * pdu, uint8_t len)
{
		if(tx_data_sign == 1){
			memcpy((uint8_t*)&m_tx_data[0], pdu, len);
		}else{
			memcpy((uint8_t*)&m_tx_data[0], (uint8_t *)&_m_tx_first[0], 2);
			tx_data_sign = 1;
		}
		
}

